package nezumi.sensing;

import nezumi.Constants;

/**
 * An IR sensor mode with only 3 distinct values corresponding to zones: A -
 * very close to the robot: very loew probability. B - close to the distance
 * indicated by the sensor: very high probability C - very far away: low
 * probability
 * 
 * @author rui
 * 
 */
public class IRDiscreteSensorModel implements ISensorModel {

	protected static final float A_PROB = 0.45f;
	protected static final float B_PROB = 0.475f;
	protected static final float C_PROB = 0.535f;
	protected static final float D_PROB = -1.0f;

	public double computeProb(final double reading, final double dist) {

		if (dist < 1 / (reading + Constants.OBST_NOISE)) // zone A
			return A_PROB;
		else if (dist < (1 / reading) - Constants.MIN_WALL_WIDTH) // zone B
			return B_PROB;
		else if (dist < (1 / reading) + Constants.MIN_WALL_WIDTH) // zone C
			return C_PROB;
		else
			return D_PROB; // zone D
	}

}
